#include "data_struct/base/DataSet.h"
#include "modules/MonitorModule/SelfTestMonitor.h"
#include "modules/CtrlModule/ActivePlanner/SelfTestPlanner.h"
#include "lib/Time.h"
#include <map>

namespace behavior_controller
{
#define cycle 0.5
SelfTestMonitor self_test_monitor;

void SelfTestMonitor::run()
{
    double time_now = Time::Now();
    double diff_time = 0;
    bool passed = true;
    static std::map<std::string, double> cycle_map = {{"battery",           cycle},
                                                      {"motor",             cycle},
                                                      {"imu",               cycle},
                                                      {"laser",             cycle},
                                                      {"bumper",            cycle},
                                                      {"off_ground_switch", cycle},
                                                      {"margin",            cycle},
                                                      {"cliff",             cycle}};
//    self_test_planner.self_test_data["battery"] = 0;
//    self_test_planner.self_test_data["motor"] = 0;
//    self_test_planner.self_test_data["imu"] = 0;
//    self_test_planner.self_test_data["laser"] = 0;
//    self_test_planner.self_test_data["bumper"] = 0;
//    self_test_planner.self_test_data["off_ground_switch"] = 0;
//    self_test_planner.self_test_data["margin"] = 0;
//    self_test_planner.self_test_data["cliff"] = 0;
    if (first)
    {
        last_time = time_now;
        first = false;
        SELT_TEST_RESULT = false;
        DataSet::ctrl_state = MAP_POSE_CHECK_PLAN;
        /*  cycle_map["battery"]=
         cycle_map["motor"]=
         cycle_map["off_ground_switch"]=
         cycle_map["imu"]=0.01;
         cycle_map["laser"]=
         cycle_map["bumper"]=
         cycle_map["margin"]=
         cycle_map["cliff"]= */
        std::cout << "self test first !" << std::endl;
        return;
    }
    if (time_now - last_time < 1)
    {
        SELT_TEST_RESULT = false;
        return;
    }

    /*if( (time_now- DataSet::battery_data.GetRealTime()) >cycle_map["battery"] )
    {
        self_test_planner.self_test_data["battery"]=1;
        std::cout<< "battery data error !"<< std::endl;
        passed=false;
    }*/

    /*if( (time_now- DataSet::motor_data.GetRealTime()) >cycle_map["motor"] )
    {
        self_test_planner.self_test_data["motor"]=1;
        std::cout<< "motor data error !"<< std::endl;
        passed=false;
    }*/

    /*if( (time_now- DataSet::off_ground_switch.GetRealTime()) > cycle_map["off_ground_switch"] )
   {
       self_test_planner.self_test_data["off_ground_switch"]=1;
       std::cout<< "off_ground_switch data error !"<< std::endl;
       passed=false;
   }*/
    diff_time = time_now - DataSet::imu_data.GetRealTime();
    if (diff_time > cycle_map["imu"])
    {
        self_test_planner.self_test_data["imu"] = 1;
        std::cout << "imu_diff_time= " << diff_time;
        passed = false;
    }

    diff_time = time_now - DataSet::scan_point_cloud.GetRealTime();
    if (diff_time > cycle_map["laser"])
    {
        self_test_planner.self_test_data["laser"] = 1;
        std::cout << " laser_diff_time= " << diff_time;
        passed = false;
    }

    diff_time = time_now - DataSet::bumper_data.GetRealTime();
    if (diff_time > cycle_map["bumper"])
    {
        self_test_planner.self_test_data["bumper"] = 1;
        std::cout << " bumper_diff_time= " << diff_time;
        passed = false;
    }

    diff_time = time_now - DataSet::margin_data.GetRealTime();
    if (diff_time > cycle_map["margin"])
    {
        self_test_planner.self_test_data["margin"] = 1;
        std::cout << " margin_diff_time= " << diff_time;
        passed = false;
    }

    diff_time = time_now - DataSet::cliff_data.GetRealTime();
    if (diff_time > cycle_map["cliff"])
    {
        self_test_planner.self_test_data["cliff"] = 1;
        std::cout << " cliff_diff_time= " << diff_time << std::endl;
        passed = false;
    }

    if (passed)
    {
        std::cout << "self test passed!" << std::endl;
        SELT_TEST_RESULT = true;
    }
    else
    {
//        std::cout << "self test failed!" << std::endl;
        SELT_TEST_RESULT = false;
    }
}
}